-
-
Notifications
You must be signed in to change notification settings - Fork 345
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add MPU6050 library and sample code #2655
Conversation
PR Summary
|
0f7ed88
to
dbbb278
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A few thoughts
Serial.print("a/g:\t"); | ||
Serial.print(ax); | ||
Serial.print("\t"); | ||
Serial.print(ay); | ||
Serial.print("\t"); | ||
Serial.print(az); | ||
Serial.print("\t"); | ||
Serial.print(gx); | ||
Serial.print("\t"); | ||
Serial.print(gy); | ||
Serial.print("\t"); | ||
Serial.println(gz); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Or Serial << "a/g:\t" << ax << "\t" << ay << "\t" << az << "\t" << gx << "\t" << gy << "\t" << gz << endl;
z /= m; | ||
} | ||
|
||
VectorFloat getNormalized() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
const
} | ||
}; | ||
|
||
class VectorInt16 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
VectorInt16
and VectorFloat
are essentially duplicates; use class template.
template <typename T> class Vector
{
public:
T x;
T y;
T z;
Vector(): x{}, y{}, z{}
{
}
Vector(T nx, T ny, T nz): x(nx), y(ny), z(nz)
{
}
// Explicit copy constructor
Vector(const Vector& other): x(other.x), y(other.y), z(other.z)
{
}
float getMagnitude()
{
return sqrt(x * x + y * y + z * z);
}
void normalize()
{
auto m = getMagnitude();
// MUST check for divide-by-zero
if(m) {
x /= m;
y /= m;
z /= m;
}
}
Vector getNormalized()
{
Vector r(*this);
r.normalize();
return r;
}
// NOTE: Use reference parameter, not pointer
void rotate(const Quaternion& q)
{
// http://www.cprogramming.com/tutorial/3d/quaternions.html
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
// ^ or:
// http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
// P_out = q * P_in * conj(q)
// - P_out is the output vector
// - q is the orientation quaternion
// - P_in is the input vector (a*aReal)
// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z],
// q*=[w,-x,-y,-z])
Quaternion p(0, x, y, z);
// quaternion multiplication: q * p, stored back in p
p = q.getProduct(p);
// quaternion multiplication: p * conj(q), stored back in p
p = p.getProduct(q.getConjugate());
// p quaternion is now [0, x', y', z']
x = p.x;
y = p.y;
z = p.z;
}
Vector getRotated(const Quaternion& q) const
{
Vector r(*this);
r.rotate(q);
return r;
}
};
using VectorInt16 = Vector<int16_t>;
using VectorFloat = Vector<float>;
w * q.z + x * q.y - y * q.x + z * q.w); // new z | ||
} | ||
|
||
Quaternion getConjugate() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Quaternion getConjugate() const
z = nz; | ||
} | ||
|
||
Quaternion getProduct(Quaternion q) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Quaternion getProduct(const Quaternion& q)
Use pass-by-reference instead of copy
return Quaternion(w, -x, -y, -z); | ||
} | ||
|
||
float getMagnitude() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
float getMagnitude() const
Quaternion() | ||
{ | ||
w = 1.0f; | ||
x = 0.0f; | ||
y = 0.0f; | ||
z = 0.0f; | ||
} | ||
|
||
Quaternion(float nw, float nx, float ny, float nz) | ||
{ | ||
w = nw; | ||
x = nx; | ||
y = ny; | ||
z = nz; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use static initialization in constructors
Quaternion(): w(1), x(0), y(0), z(0)
{
}
Quaternion(float nw, float nx, float ny, float nz): w(nw), x(nx), y(ny), z(nz)
{
}
Also add copy constructor:
Quaternion(const Quaternion& other): w(other.w), x(other.x), y(other.y), z(other.z)
{
}
z /= m; | ||
} | ||
|
||
Quaternion getNormalized() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Quaternion getNormalized() const
|
||
Quaternion getNormalized() | ||
{ | ||
Quaternion r(w, x, y, z); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Quaternion r(*this);
mpu.initialize(); | ||
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); | ||
|
||
mainLoopTimer.initializeMs(static_cast<int>(mainLoopInterval * 1000), mainLoop).start(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
or mainLoopTimer.initializeMs<mainLoopInterval * 1000>(mainLoop).start();
as mainLoopInterval
is declared constexpr
4bc6dfc
to
ad9fe43
Compare
After a closer examination, I realized the quaternion related code is not used in this part of the library (see here). So I simply deleted it to keep the library lean. The other comments of yours I have addressed. Thank you for the feedback. Please have a look again~ |
ad9fe43
to
d22c2c8
Compare
d22c2c8
to
bc9f997
Compare
@mikee47 The build with IDF v5.0 is failing. I was able to reproduce the issue on my machine and it seems to not be related to this PR. Can you check if we have to downgrade/upgrade the pyparse version?
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is an example of code which got written with a 'C' viewpoint rather than C++. More robust, easier to manage and safer to use references instead of pointers, structures instead of long parameter lists, etc. Hopefully this all makes sense?
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
/** Default constructor, uses default I2C address. | ||
* @see MPU6050_DEFAULT_ADDRESS | ||
*/ | ||
MPU6050::MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Trivial methods should be moved to header (compiler cannot optimise what it cannot see)
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
* @see MPU6050_ADDRESS_AD0_LOW | ||
* @see MPU6050_ADDRESS_AD0_HIGH | ||
*/ | ||
MPU6050::MPU6050(uint8_t address) : devAddr{address} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Move to header
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
bool MPU6050::testConnection() | ||
{ | ||
return getDeviceID() == 0x34; | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Move to header ?
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
if(num > 3) | ||
return 0; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use braces for conditionals:
if(num > 3) {
return 0;
}
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
if(num > 3) | ||
return; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Braces
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer); | ||
return buffer[0]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Using helper
return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH);
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]); | ||
I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); | ||
return (buffer[0] >> 3) | ((buffer[1] >> 2) & 0x03); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Using helpers:
uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
uint8_t a = readByte(MPU6050_RA_SELF_TEST_A);
return (y >> 3) | ((a >> 2) & 0x03);
@@ -0,0 +1 @@ | |||
COMPONENT_DEPENDS := I2Cdev |
This comment was marked as outdated.
This comment was marked as outdated.
Sorry, something went wrong.
Sming/Libraries/MPU6050/MPU6050.h
Outdated
#define MPU6050_DMP_MEMORY_BANK_SIZE 256 | ||
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 | ||
|
||
// note: DMP code memory blocks defined at end of header file |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Remove comment (or edit to make sense)
Sming/Libraries/MPU6050/MPU6050.h
Outdated
uint8_t buffer[14] = {0}; | ||
}; | ||
|
||
#endif /* _MPU6050_H_ */ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Remove (#pragma once)
Thanks for the review, I agree the code which I copy/paste is not to the standard of Sming. I will do the refactoring when I find time. Currently I want to first prototype the balance robot I am working on:) |
@xeonqq are you ready with all changes to this PR? |
No, not yet, there are still many comments I didn't address yet. Quite some workload. I will remove the wip tag in the PR title once I am ready for another review. |
The code is not used, since MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h due to deps to freeRTOS are not included
Move trival functions to header
For simplicity and code quality, they are deleted at the moment
6d6c6c4
to
c5a7909
Compare
@mikee47 ready for review again. Sorry for the delayed PR. |
372f513
to
e832473
Compare
Does that sometimes, host emulator isn't perfect :-( Re-running the test |
Sming/Libraries/MPU6050/README.rst
Outdated
API Documentation | ||
----------------- | ||
|
||
.. doxygenclass:: MPU6050 | ||
:members: | ||
|
||
# MPU6050 Six-Axis (Gyro + Accelerometer) | ||
Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050) @ 605a740. Most of the code is the same, except: | ||
|
||
- Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code. | ||
- MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files. | ||
- Removed map function in favor of the Sming built-in one. | ||
- Adapted include path, coding style and applied clangformat | ||
- Deleted Calibration and Memory Block related code for code quality reason |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Have a look at other libraries/Components for general layout. Heading must be double-underlined (level 1) and give a good name for the library. For example:
MPU6050 Gyro / Accelerometer
============================
Follow this with the description. Note that .rst
files (restructured text) syntax differs from markdown so remember to fix links, e.g.
`jrowberg/i2cdevlib<https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>`
And put the API section last.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I took look of some, but they are not very consistent. I do get confused on how to write it properly
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah, takes a bit of getting used to but has advantages to markdown! See https://sming.readthedocs.io/en/latest/information/develop/documentation.html
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You've inadvertently reverted the FlashString
and IFS
libraries! Other than that, and the documentation change, looks great! I assume it's all working and tested on actual hardware?
fbe42ad
to
0327384
Compare
I just realized that too, and reverted the unintended change of the submodules. I tested on esp8266 and mpu6050, but only the functions used in the sample program. So I can't say for sure it is bullet proof. |
Sming/Libraries/MPU6050/README.rst
Outdated
:members: | ||
|
||
# MPU6050 Six-Axis (Gyro + Accelerometer) | ||
Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>` @ 605a740. Most of the code is the same, except: | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Almost... remove :members:
and line starting '#'. Also the URL requires __ at the end!
Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>`__ @ 605a740. Most of the code is the same, except:
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
done
b52fea9
to
4aac518
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fix small typos.
Sming/Libraries/MPU6050/MPU6050.cpp
Outdated
// Changelog: | ||
// ... - ongoing debug release | ||
|
||
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fix typo, replace PARIAL
with PARTIAL
.
Sming/Libraries/MPU6050/MPU6050.h
Outdated
// (RM-MPU-6000A-00) | ||
// Based On https://github.com/jrowberg/i2cdevlib | ||
|
||
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fix typo, replace PARIAL
with PARTIAL
.
Sming/Libraries/MPU6050/MPU6050.h
Outdated
|
||
const auto sz = sizeof(T); | ||
uint8_t buffer[sz] = {0}; | ||
//data follow big endien convention |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fix typo, replace endien
with endian
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
done
No description provided.